#include "sv2_gimbal_keyboard.h"

namespace sv2
{

  void GimbalKeyBoardCppNode::run()
  {
    int key;
    while (this->is_running())
    {
      nlohmann::json gimbal_ctl_msg = sms::def_msg("spirecv_msgs::GimbalControl");
      key = getch(); // 获取键盘输入
      switch (key)
      {
      case KEY_UP: // 上箭头
        pitch = pitch - 1;
        break;
      case KEY_DOWN: // 下箭头
        pitch = pitch + 1;
        break;
      case KEY_LEFT: // 左箭头
        yaw = yaw + 1;
        break;
      case KEY_RIGHT: // 右箭头
        yaw = yaw - 1;
        break;
      case '+':
        zoomScale = zoomScale + 0.1;
        zoomScale = std::max(1.0, std::min(10.0, zoomScale));
        break;
      case 61:
        zoomScale = zoomScale + 0.1;
        zoomScale = std::max(1.0, std::min(10.0, zoomScale));
        break;
      case '-':
        zoomScale = zoomScale - 0.1;
        zoomScale = std::max(1.0, std::min(10.0, zoomScale));
        break;
      case 27: // ESC 键，用于退出程序
        printw("退出程序\n");
        endwin(); // 结束 ncurses 模式
        exit(0);
      default:
        printw("\n");
        break;
      }
      refresh(); // 刷新屏幕

      gimbal_ctl_msg["zoom"] = zoomScale;
      gimbal_ctl_msg["angle_euler_cmd"] = {0, pitch, yaw};
      gimbal_ctl_msg["ctl_mode"] = 2;
      _gimbal_ctl_pub.publish(gimbal_ctl_msg);
    }
    endwin(); // 结束 ncurses 模式
  }
}
